#include "motor.h"

float MOTOR_ANGLE = 0;

void move_forward()
{
    IoTGpioSetOutputVal(M1_GPIO, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(M2_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M3_GPIO, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(M4_GPIO, IOT_GPIO_VALUE0);
}

void move_back()
{
    IoTGpioSetOutputVal(M1_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M2_GPIO, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(M3_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M4_GPIO, IOT_GPIO_VALUE1);
}

// 90为正，小于90位左，大于90为右
void move_angle(float angle)
{
    int i;
    unsigned int duty = (-100 * angle / 9) + 1500;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(CPWM_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(CPWM_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(CPWM_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    }
}

void move_stop()
{
    IoTGpioSetOutputVal(M1_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M2_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M3_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M4_GPIO, IOT_GPIO_VALUE0);
}

void set_speed(unsigned int speed)
{
    int i;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(MPWM_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(MPWM_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(speed);
        IoTGpioSetOutputVal(MPWM_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - speed);
    }
}

void Motor_GpioInit()
{
    IoTGpioInit(M1_GPIO);
    IoTGpioInit(M2_GPIO);
    IoTGpioInit(M3_GPIO);
    IoTGpioInit(M4_GPIO);
    IoTGpioInit(MPWM_GPIO);
    IoTGpioInit(CPWM_GPIO);
    IoSetFunc(M1_GPIO, 0);
    IoSetFunc(M2_GPIO, 0);
    IoSetFunc(M3_GPIO, 0);
    IoSetFunc(M4_GPIO, 0);
    IoSetFunc(MPWM_GPIO, 0);
    IoSetFunc(CPWM_GPIO, 0);
    IoTGpioSetDir(M1_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(M2_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(M3_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(M4_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(MPWM_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(CPWM_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(M1_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M2_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M3_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(M4_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(MPWM_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(CPWM_GPIO, IOT_GPIO_VALUE0);
}

static void *MotorTask(void *parame)
{
    (void)parame;
    printf("Motor task start\r\n");
    move_forward();
    while (1)
    {
        set_speed(20000);
        move_angle(MOTOR_ANGLE);
        osDelay(1);
    }
}

static void MotorEntry(void)
{
    osThreadAttr_t attr;

    Motor_GpioInit();
    IoTGpioInit(8);
    IoSetFunc(8, 0);
    IoTGpioSetDir(8, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(8, IOT_GPIO_VALUE1);

    attr.name = "MotorTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = MOTOR_TASK_STACK_SIZE;
    attr.priority = osPriorityNormal;

    if (osThreadNew((osThreadFunc_t)MotorTask, NULL, &attr) == NULL)
    {
        printf("[MotorEntry] Falied to create MotorTask!\n");
    }
}

APP_FEATURE_INIT(MotorEntry);